﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Kinect;
using Biomedica.Graphics;
using System.Windows;
using System.Globalization;
using System.Linq.Expressions;
using System.ComponentModel;
using KinectWindows.Math;

namespace NuiDicomPlayer.VirtualMouse.Kinect
{
    public class Filter
    {
        int framcount;
        Vector2 savedFilterdJointPostion;
        Vector2 savedTrend;
        Vector2 savedBasePostion;
        public float TrendSmoothFactoe { set; get; }
        public float JitterRadious { set; get; }
        public float DataSmothingFactor { set; get; }
        public float PredicationFacrtor { set; get; }
        public float GlobalSmooth { set; get; }
        private HandPosition activeHand;
        public HandPosition ActiveHand
        {
            get
            {
                return activeHand;
            }

            set
            {
                activeHand = value;
              //  OnPropertyChanged(() => ActiveHand);
            }
        }
        public Filter()
        {
            TrendSmoothFactoe = 0.5f;
            JitterRadious = 0.001f;
            DataSmothingFactor = 0.5f;
            PredicationFacrtor = 1.0f;
            savedFilterdJointPostion = Vector2.Zero;
            savedTrend = Vector2.Zero;
            savedBasePostion = Vector2.Zero;
            framcount = 0;
        }
        public Vector2 FilterJointPostion(KinectSensor sensor, Vector2 joint)
        {

            Vector2 filteredJointPostion = Vector2.Zero;
            Vector2 differanceVector = Vector2.Zero;
            Vector2 currentTrend = Vector2.Zero;
            Vector2 baseJointPostion = joint;// Convert(sensor, joint.Position);
            Vector2 previousFilterdJointPostion = savedFilterdJointPostion;
            Vector2 previousTrens = savedTrend;

            Vector2 previousBaseJointPostion = savedBasePostion;

            switch (framcount)
            {
                case (0):
                    filteredJointPostion = baseJointPostion;
                    currentTrend = Vector2.Zero;
                    break;
                case (1):
                    filteredJointPostion = (baseJointPostion + previousBaseJointPostion) * 0.5;
                    differanceVector = filteredJointPostion - previousBaseJointPostion;
                    currentTrend = differanceVector * TrendSmoothFactoe + previousTrens * (1.0f - TrendSmoothFactoe);
                    break;

                default:

                    differanceVector = baseJointPostion - previousFilterdJointPostion;


                    float distance = differanceVector.Distance();
                    if (distance <= JitterRadious)
                        filteredJointPostion = baseJointPostion * (distance / JitterRadious) + previousFilterdJointPostion * (1.0f) - (distance / JitterRadious);
                    else
                        filteredJointPostion = baseJointPostion;

                    filteredJointPostion = filteredJointPostion * (1.0f - DataSmothingFactor) + (previousFilterdJointPostion + previousTrens) * DataSmothingFactor;
                    differanceVector = filteredJointPostion - previousFilterdJointPostion;
                    currentTrend = differanceVector * TrendSmoothFactoe + previousTrens * (1.0f - TrendSmoothFactoe);
                    break;

            }
            Vector2 potentialNewPOstion = new Vector2(filteredJointPostion.X + currentTrend.X * PredicationFacrtor, filteredJointPostion.Y + currentTrend.Y * PredicationFacrtor);
            savedBasePostion = baseJointPostion;
            savedFilterdJointPostion = filteredJointPostion; ;
            savedTrend = currentTrend;
            framcount++;
            return potentialNewPOstion;

        }

        private void SkeletonUpdated(Skeleton skeleton)
        {
            if (this.ActiveHand == null)
            {
                this.ActiveHand = new HandPosition();
            }

            int leftX, leftY, rightX, rightY;
            Joint leftHandJoint = skeleton.Joints[JointType.HandLeft];
            Joint rightHandJoint = skeleton.Joints[JointType.HandRight];

            float leftZ = leftHandJoint.Position.Z;
            float rightZ = rightHandJoint.Position.Z;

            ScaleXY(skeleton.Joints[JointType.ShoulderCenter], false, leftHandJoint, out leftX, out leftY);
            ScaleXY(skeleton.Joints[JointType.ShoulderCenter], true, rightHandJoint, out rightX, out rightY);

            if (leftHandJoint.TrackingState == JointTrackingState.Tracked && leftZ < rightZ && leftY < SystemParameters.PrimaryScreenHeight)
            {
                this.ActiveHand.IsLeft = true;
                this.ActiveHand.X = leftX;
                this.ActiveHand.Y = leftY;
                //this.HitTestHand(this.ActiveHand);
            }
            else if (rightHandJoint.TrackingState == JointTrackingState.Tracked && rightY < SystemParameters.PrimaryScreenHeight)
            {
                this.ActiveHand.IsLeft = false;
                this.ActiveHand.X = rightX;
                this.ActiveHand.Y = rightY;
                //this.HitTestHand(this.ActiveHand);
            }
            else
            {
                if (this.activeHand != null && this.activeHand.CurrentElement != null)
                {
                    this.ActiveHand.X = -1;
                    this.ActiveHand.Y = -1;
                   // HandInputEventArgs args = new HandInputEventArgs(HandLeaveEvent, this.ActiveHand.CurrentElement, this.ActiveHand);
                   // this.activeHand.CurrentElement.RaiseEvent(args);
                   // this.activeHand.CurrentElement = null;
                }

                this.ActiveHand = null;
            }

            if (this.ActiveHand == null)
            {
                return;
            }

            if (this.ActiveHand.CurrentElement != null)
            {
                if (ScaleY(skeleton.Joints[JointType.ElbowLeft]) > ScaleY(skeleton.Joints[JointType.HandLeft])
                    || ScaleY(skeleton.Joints[JointType.ElbowRight]) > ScaleY(skeleton.Joints[JointType.HandRight]))
                {
                   // HandInputEventArgs args = new HandInputEventArgs(HandRaisedEvent, this.ActiveHand.CurrentElement, this.ActiveHand);
                  //  this.ActiveHand.CurrentElement.RaiseEvent(args);
                }
            }
        }

        private static double ScaleY(Joint joint)
        {
            double y = ((SystemParameters.PrimaryScreenHeight / 0.4) * -joint.Position.Y) +
                       (SystemParameters.PrimaryScreenHeight / 2);
            return y;
        }

        private static void ScaleXY(Joint shoulderCenter, bool rightHand, Joint joint, out int scaledX, out int scaledY)
        {
            double screenWidth = SystemParameters.PrimaryScreenWidth;

            double x = 0;
            double y = ScaleY(joint);

            // if rightHand then place shouldCenter on left of screen
            // else place shouldCenter on right of screen
            if (rightHand)
            {
                x = (joint.Position.X - shoulderCenter.Position.X) * screenWidth * 2;
            }
            else
            {
                x = screenWidth - ((shoulderCenter.Position.X - joint.Position.X) * (screenWidth * 2));
            }


            if (x < 0)
            {
                x = 0;
            }
            else if (x > screenWidth - 5)
            {
                x = screenWidth - 5;
            }

            if (y < 0)
            {
                y = 0;
            }

            scaledX = (int)x;
            scaledY = (int)y;
        }

        private Vector2 Convert(KinectSensor sensor, SkeletonPoint skeletonPoint)
        {

            int width = 0;
            int height = 0;
            int x = 0;
            int y = 0;

            if (sensor.DepthStream.IsEnabled)
            {
                var colorPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(skeletonPoint, KinectSensor.KinectSensors[0].DepthStream.Format);
                x = colorPoint.X;
                y = colorPoint.Y;

                switch (sensor.DepthStream.Format)
                {
                    case DepthImageFormat.Resolution80x60Fps30:
                        width = 80;
                        height = 60;
                        break;
                    case DepthImageFormat.Resolution640x480Fps30:
                        width = 640;
                        height = 480;
                        break;
                    case DepthImageFormat.Resolution320x240Fps30:
                        width = 320;
                        height = 240;
                        break;

                }

            }

            return new Vector2((float)x / width, (float)y / height);


        }

        public class HandPosition : INotifyPropertyChanged
        {
            private bool isInteracting;
            private bool isLeft;
            private int x;
            private int y;

            public HandPosition()
            {
               // this.MagneticField = Settings.Default.MagneticField;
            }

            #region INotifyPropertyChanged Members

            public event PropertyChangedEventHandler PropertyChanged;

            #endregion

            public double MagneticField { get; set; }

            /// <summary>
            /// The PlayerId that the hand belongs to.
            /// </summary>
            public int PlayerId { get; set; }

            /// <summary>
            /// The current element within the application that the hand is over.
            /// </summary>
            public IInputElement CurrentElement { get; set; }

            public bool Magnetized
            {
                get
                {
                    return this.MagnetizedHorizontally || this.MagnetizedVertically;
                }

                set
                {
                    this.MagnetizedVertically = value;
                    this.MagnetizedHorizontally = value;
                    this.OnPropertyChanged(() => this.Magnetized);
                }
            }

            public bool MagnetizedHorizontally { get; set; }

            public bool MagnetizedVertically { get; set; }

            /// <summary>
            /// The X coordinate of the hand within the application, scaled to the screen size.
            /// </summary>
            public int X
            {
                get
                {
                    return this.x;
                }

                set
                {
                    if (this.MagnetizedHorizontally)
                    {
                        if (Math.Abs(value - this.x) < this.MagneticField)
                        {
                            return;
                        }
                    }

                    this.MagnetizedHorizontally = false;
                    this.x = value;
                    this.OnPropertyChanged(() => this.X);
                }
            }

            /// <summary>
            /// The Y coordinate of the hand within the application, scaled to the screen size.
            /// </summary>
            public int Y
            {
                get
                {
                    return this.y;
                }

                set
                {
                    if (this.MagnetizedVertically)
                    {
                        if (Math.Abs(value - this.y) < this.MagneticField)
                        {
                            return;
                        }
                    }

                    this.MagnetizedVertically = false;
                    this.y = value;
                    this.OnPropertyChanged(() => this.Y);
                }
            }

            /// <summary>
            /// Used to set whether the hand is the left or right hand. True = Left, False = Right.
            /// </summary>
            public bool IsLeft
            {
                get
                {
                    return this.isLeft;
                }

                set
                {
                    this.isLeft = value;
                    this.OnPropertyChanged(() => this.IsLeft);
                }
            }

            /// <summary>
            /// Used to set whether or not the hand is interacting with a UI element.
            /// </summary>
            public bool IsInteracting
            {
                get
                {
                    return this.isInteracting;
                }

                set
                {
                    this.isInteracting = value;
                    this.OnPropertyChanged(() => this.IsInteracting);
                }
            }

            public override bool Equals(object obj)
            {
                var other = obj as HandPosition;
                if (other != null)
                {
                    return this.PlayerId.Equals(other.PlayerId) && this.IsLeft.Equals(other.IsLeft);
                }

                return false;
            }

            public override int GetHashCode()
            {
                string hash = this.PlayerId.ToString(CultureInfo.InvariantCulture) + this.IsLeft.ToString();
                return hash.GetHashCode();
            }

            private void OnPropertyChanged<T>(Expression<Func<T>> expression)
            {
                if (this.PropertyChanged == null)
                {
                    return;
                }

                var body = (MemberExpression)expression.Body;
                string propertyName = body.Member.Name;
                var args = new PropertyChangedEventArgs(propertyName);
                this.PropertyChanged(this, args);
            }
        }

    }
}
